function [tr_param ,index_current_action]= update_action(xtrue, action, index_current_action,...
                                                         tr_param, MAX_T, MAX_R);
%
% INPUTS:
%   xtrue - true position
%   action - transformation sequence
%   index_current_action - index to current waypoint
%   tr_param - transformation parameters
%   RATE_ROTATION - max steering rate (rad/s)
%   MAX_ROTATION - max steering angle (rad)

T_THRES = 5;
R_THRES = 1*pi/180;
flag_t_done = 0;
flag_r_done = 0;

% Set current action
current_action = action(:, index_current_action);

% Check if current action is complete
% first check if translation command is complete
% if xtrue(T) is near to given translation

dis = sqrt((xtrue(1)-current_action(1))^2 + (xtrue(2)-current_action(2))^2 + (xtrue(3)-current_action(3))^2);

if ( dis  ) < T_THRES
    %  - done
    for i=1:3
        tr_param(i) = 0;
    end
    flag_t_done = 1;
else
    % if not, set tr_param(T)
    for i=1:3
        tr_param(i) = (current_action(i) - xtrue(i));%/dis;
        if abs(tr_param(i)) > MAX_T
            tr_param(i) = MAX_T*sign(tr_param(i));
        end
    end

end

if flag_t_done == 1
    % second check if rotation command is complete
    % if xtrue(R) is near to given rotation

    for i = 4:6
        if abs(pi_to_pi(xtrue(i))-pi_to_pi(current_action(i))) < R_THRES
            %  rotate i done
            tr_param(i) = 0;
        else
            % if not, set tr_param(R)            
            delta_r = pi_to_pi(current_action(i)) - xtrue(i);

            if abs(delta_r) > MAX_R
                tr_param(i) = MAX_R*sign(delta_r);
            else
                tr_param(i) = delta_r;
            end
        end
    end
    
  if (pi_to_pi(abs(xtrue(4)-current_action(4))) < R_THRES & ...
      pi_to_pi(abs(xtrue(5)-current_action(5))) < R_THRES & ...
      pi_to_pi(abs(xtrue(6)-current_action(6))) < R_THRES)
          %  ALL done
          flag_r_done = 1;    
  end
    
%     
%     if (pi_to_pi(abs(xtrue(4)-current_action(4))) < R_THRES & ...
%         pi_to_pi(abs(xtrue(5)-current_action(5))) < R_THRES & ...
%         pi_to_pi(abs(xtrue(6)-current_action(6))) < R_THRES)
%         %  - done
%         for i=4:6
%             tr_param(i) = 0;
%         end
%         flag_r_done = 1;
%     else
%         % if not, set tr_param(R)
%         for i=4:6
%             
% %            pi_to_pi(current_action(i)) - xtrue(i)
% 
%             delta_r = pi_to_pi(current_action(i)) - xtrue(i);
%             if abs(delta_r) > MAX_RR
%                 delta_r = MAX_RR * sign(delta_r);
%             end
%             
%             if abs(delta_r) > MAX_R
%                 tr_param(i) = MAX_R*sign(delta_r);
%             end
% 
%             if abs(delta_r) < MAX_R
%                 tr_param(i) = delta_r;
%             end
%                
%             
% %             tr_param(i) = pi_to_pi(current_action(i)) - xtrue(i);
% %             if abs(tr_param(i)) > MAX_R
% %                 tr_param(i) = MAX_R*sign(tr_param(i));
% %             end
%         end
%     end
end

if (flag_t_done + flag_r_done) ==2
    index_current_action = index_current_action + 1; % switch to next
    if index_current_action > size(action,2) % reached final action, flag and return
        index_current_action = 0;
        return;
    end    
    current_action = action(:,index_current_action); % next action
end

